package com.gm.sample;

import Jama.Matrix;
import com.gm.math.KalmanFilter;
import com.gm.tools.Gen;
import org.jfree.chart.ChartFactory;
import org.jfree.chart.ChartUtilities;
import org.jfree.chart.JFreeChart;
import org.jfree.chart.plot.PlotOrientation;
import org.jfree.data.category.CategoryDataset;
import org.jfree.data.category.DefaultCategoryDataset;

import java.io.FileNotFoundException;
import java.io.FileOutputStream;
import java.io.IOException;

/**
 * Created by Mouse on 2017/8/10.
 */
public class Test2 {

    String path = "D:\\BarChart2.png";

    KalmanFilter kalmanFilter = new KalmanFilter(2, 1, 1);

    public Test2() {
    }

    public void toFile() {
        CategoryDataset dataset = getCategoryDataset();

        JFreeChart chart = ChartFactory.createLineChart(
                "test", "time", "distance",
                dataset,
                PlotOrientation.VERTICAL,
                true,
                false,
                false
        );

        FileOutputStream fos = null;
        try {
            fos = new FileOutputStream(path);
            ChartUtilities.writeChartAsPNG(fos, chart, 1920, 1080, null);
        } catch (FileNotFoundException e) {
            e.printStackTrace();
        } catch (IOException e) {
            e.printStackTrace();
        } finally {
            try {
                fos.close();
            } catch (Exception e) {
            }
        }
    }

    public CategoryDataset getCategoryDataset() {

        DefaultCategoryDataset dataset = new DefaultCategoryDataset();

        double a[][] = {
                {1, 1},
                {0, 1}
        };
        double b[][] = {
                {0.25},
                {1}
        };
        double h[][] = {
                {1, 0}
        };
        double p[][] = {
                {0, 0},
                {0, 0}
        };
        double q[][] = {
                {0, 0},
                {0, 0.01}
        };
        double r[][] = {
                {10}
        };
        double x[][] = {
                {0},
                {500}
        };
        double u[][] = {
                {-9.8}
        };


        kalmanFilter.setA(new Matrix(a));
        kalmanFilter.setB(new Matrix(b));
        kalmanFilter.setH(new Matrix(h));
        kalmanFilter.setP(new Matrix(p));

        kalmanFilter.setQ(new Matrix(q));
        kalmanFilter.setR(new Matrix(r));

        kalmanFilter.setX_k(new Matrix(x));
        kalmanFilter.setU_k(new Matrix(u));

        kalmanFilter.getX_k().print(4, 2);
        Matrix z_k = new Matrix(1, 1);
        for (int i = 1; i <= 100; i++) {
            try {
                z_k.getArray()[0][0] = Gen.toDouble(500 * (i - 1) - 5 * Math.pow((i - 1), 2), 1000);
                kalmanFilter.update(z_k);
                Matrix value = kalmanFilter.getX_k();
                value.print(4, 2);
                dataset.addValue(z_k.getArray()[0][0], "Z_a", "" + i);
                dataset.addValue(value.getArray()[0][0], "X_a", "" + i);
            } catch (Exception e) {
                e.printStackTrace();
            }
        }
        return dataset;
    }
}
